function [trajectory, landmarks, stats, true_traj_out] = run_ekf_slam(true_traj, obstacles, dt, Q, R, laser_range, laser_fov, num_beams, laser_noise, show_progress, v_base, omega_base)
% 运行EKF-SLAM算法
% 输入:
%   true_traj - 真实轨迹 [3 x steps]（如果为空，则实时生成）
%   obstacles - 障碍物位置 [N x 2]
%   dt - 时间步长
%   Q - 过程噪声协方差
%   R - 观测噪声协方差
%   laser_range - 激光最大距离
%   laser_fov - 激光视场角
%   num_beams - 激光束数量
%   laser_noise - 测距噪声标准差
%   show_progress - 是否显示进度（默认false）
%   v_base, omega_base - 动态模式下的速度参数（可选）
% 输出:
%   trajectory - 估计轨迹 [3 x steps]
%   landmarks - 路标数据结构
%   stats - 统计信息
%   true_traj_out - 实际使用的真实轨迹（动态模式下返回）

    if nargin < 10
        show_progress = false;
    end
    
    % 判断是否需要实时生成轨迹（动态避障模式）
    generate_trajectory = isempty(true_traj);
    
    if generate_trajectory
        % 动态模式：需要实时生成轨迹
        if nargin < 12
            error('动态模式需要提供 v_base 和 omega_base 参数');
        end
        config = get_slam_config();
        steps = config.steps;
        % 随机初始方向
        initial_theta = 0;  % 固定初始方向：水平向右
        true_state = [config.initial_state(1); config.initial_state(2); initial_theta];
        fprintf('  随机初始方向: %.2f rad (%.1f度)\n', initial_theta, rad2deg(initial_theta));
        
        % 初始化运动控制状态
        motion_state = struct();
        motion_state.boundary_avoiding = false;
        motion_state.obstacle_avoiding = false;
        motion_state.avoidance_timer = 0;
        motion_state.angle_diff = 0;
        motion_state.v = v_base;
        motion_state.omega = omega_base;
        
        % 预分配轨迹数组
        true_traj = zeros(3, steps);
    else
        % 固定模式：使用预生成的轨迹
        steps = size(true_traj, 2);
    end
    
    % 初始化
    if generate_trajectory
        estimated_state = true_state;  % 动态模式：从真实状态开始
    else
        estimated_state = [25; 25; 0];  % 固定模式：从固定位置开始
    end
    P = diag([0.01, 0.01, 0.001]).^2;
    trajectory = zeros(3, steps);
    
    % 路标管理
    landmark_map = containers.Map('KeyType', 'int32', 'ValueType', 'any');
    
    % 主循环
    progress_interval = max(1, round(steps/20));
    
    for t = 1:steps
        % 显示进度
        if show_progress && (mod(t, progress_interval) == 0 || t == 1 || t == steps)
            percent = round(100 * t / steps);
            n_landmarks = landmark_map.Count;
            fprintf('  [%3d%%] 步骤 %4d/%4d | 路标数: %3d\r', percent, t, steps, n_landmarks);
            if t == steps
                fprintf('\n');
            end
        end
        
        % 生成控制输入
        if generate_trajectory
            % 动态模式：实时生成控制输入
            if t == 1
                control = [0; 0];
            else
                [v, omega, motion_state] = safe_motion_controller(true_state, obstacles, motion_state, v_base, omega_base, dt);
                control = [v; omega];
            end
            
            % 执行真实运动
            true_state = motion_model(true_state, control, dt, Q);
            
            % 安全检查
            [still_collision, ~] = check_collision(true_state, obstacles, 0.5);
            if still_collision && t > 1
                true_state = true_traj(:, t-1);
                motion_state.omega = 0.5 * sign(randn());
                motion_state.obstacle_avoiding = true;
                motion_state.avoidance_timer = 20;
            end
            
            % 记录真实轨迹
            true_traj(:, t) = true_state;
        else
            % 固定模式：从预生成轨迹中提取控制
            if t == 1
                control = [0; 0];
            else
                dx = true_traj(1, t) - true_traj(1, t-1);
                dy = true_traj(2, t) - true_traj(2, t-1);
                dtheta = wrapToPi(true_traj(3, t) - true_traj(3, t-1));
                v = sqrt(dx^2 + dy^2) / dt;
                omega = dtheta / dt;
                control = [v; omega];
            end
        end
        
        % EKF预测
        [estimated_state, P] = ekf_predict(estimated_state, P, control, dt, Q);
        
        % 激光观测（使用当前真实位置）
        current_true_pose = true_traj(:, t);
        [observations, ~] = observation_model_laser(current_true_pose, obstacles, ...
                                        laser_range, laser_fov, num_beams, laser_noise);
        
        % 数据关联与更新
        if ~isempty(observations)
            % 提取已知路标信息
            n_landmarks = (length(estimated_state) - 3) / 2;
            landmarks = [];
            landmark_ids = [];
            P_landmarks = {};
            
            if n_landmarks > 0
                keys = landmark_map.keys();
                for k = 1:length(keys)
                    lm_id = keys{k};
                    lm_idx = landmark_map(lm_id);
                    m_idx = 3 + (lm_idx - 1) * 2;
                    landmarks = [landmarks; estimated_state(m_idx+1:m_idx+2)'];
                    landmark_ids = [landmark_ids; lm_id];
                    % 提取路标协方差
                    P_lm = P(m_idx+1:m_idx+2, m_idx+1:m_idx+2);
                    P_landmarks{end+1} = P_lm;
                end
            end
            
            % 数据关联
            [associations, is_new] = data_association(observations, estimated_state(1:3), ...
                                                      landmarks, landmark_ids, P_landmarks, R, []);
            
            % 处理每个观测
            for i = 1:size(observations, 2)
                z = observations(:, i);
                
                if is_new(i)
                    % 初始化新路标
                    [estimated_state, P] = initialize_landmark(estimated_state, P, z, R);
                    new_id = landmark_map.Count + 1;
                    landmark_map(new_id) = (length(estimated_state) - 3) / 2;
                else
                    % 更新已知路标
                    assoc_idx = associations(i);
                    lm_id = landmark_ids(assoc_idx);
                    landmark_idx = landmark_map(lm_id);
                    [estimated_state, P] = ekf_update(estimated_state, P, z, landmark_idx, R);
                end
            end
        end
        
        trajectory(:, t) = estimated_state(1:3);
    end
    
    % 返回路标
    landmarks = struct();
    landmarks.map = landmark_map;
    landmarks.positions = [];
    landmarks.state = estimated_state;
    
    % 提取所有路标的位置
    if landmark_map.Count > 0
        keys_list = cell2mat(landmark_map.keys());
        for i = 1:length(keys_list)
            lm_id = keys_list(i);
            lm_idx = landmark_map(lm_id);
            m_idx = 3 + (lm_idx - 1) * 2;
            lm_pos = estimated_state(m_idx+1:m_idx+2);
            landmarks.positions = [landmarks.positions, lm_pos];
        end
    end
    
    % 计算统计信息
    position_error = sqrt(sum((true_traj(1:2, :) - trajectory(1:2, :)).^2, 1));
    stats = struct();
    stats.errors = position_error;
    stats.avg_error = mean(position_error);
    stats.max_error = max(position_error);
    stats.n_landmarks = landmarks.map.Count;
    
    % 返回真实轨迹（动态模式）
    if generate_trajectory
        true_traj_out = true_traj;
    else
        true_traj_out = [];
    end
end
